#include <util/msg.h>
#include <string.h>

#include "../../matrix/matrix.h"
#include "filters.h"

/**
 * This function computes 1st and 2nd derivatives of an image using the 5-tap
 * coefficients given by Farid and Simoncelli.  The results are significantly
 * more accurate than MATLAB's GRADIENT function on edges that are at angles
 * other than vertical or horizontal. This in turn improves gradient orientation
 * estimation enormously.
 * 
 * Hany Farid and Eero P. Simoncelli (2004). Differentiation of Discrete Multidimensional Signals
 * IEEE TRANSACTIONS ON IMAGE PROCESSING, VOL. 13, NO. 4, APRIL 2004
 * 
 */


/* ================================= 5x5 ==================================== */
// Optimal 1st derivative 5x5 farid and simoncelli filter components
static real _5_1st_p[5] = {0.037659, 0.249153, 0.426375, 0.249153, 0.037659};
static real _5_1st_d1[5] = {0.109604, 0.276691, 0.000000, -0.276691, -0.109604};
static real _5_dx[25] = {
	0.0041, 0.0104, 0, -0.0104, -0.0041,
	0.0273, 0.0689, 0, -0.0689, -0.0273,
	0.0467, 0.1180, 0, -0.1180, -0.0467,
	0.0273, 0.0689, 0, -0.0689, -0.0273,
	0.0041, 0.0104, 0, -0.0104, -0.0041
};
static real _5_dy[25] = {
	+0.0041, +0.0273, +0.0467, +0.0273, +0.0041,
	+0.0104, +0.0689, +0.1180, +0.0689, +0.0104,
	00.0000, 00.0000, 00.0000, 00.0000, 00.0000,
	-0.0104, -0.0689, -0.1180, -0.0689, -0.0104,
	-0.0041, -0.0273, -0.0467, -0.0273, -0.0041
};
static real _5_dxy[25] = {
	+0.0037, +0.0094, 0.0000, -0.0094, -0.0037,
	+0.0094, +0.0236, 0.0000, -0.0236, -0.0094,
	+0.0000, +0.0000, 0.0000, +0.0000, +0.0000,
	-0.0094, -0.0236, 0.0000, +0.0236, +0.0094,
	-0.0037, -0.0094, 0.0000, +0.0094, +0.0037
};

// Optimal 2st derivative 5x5 farid and simoncelli filter components. Interpolation to 1st derivative.
static real _5_2st_p[5] = {0.030320, 0.249724, 0.439911, 0.249724, 0.030320};
static real _5_2st_d1[5] = {0.104550, 0.292315, 0.000000, -0.292315, -0.104550};
static real _5_2st_d2[5] = {0.232905, 0.002668, -0.471147, 0.002668, 0.232905};
static real _5_dxx[25] = {
	0.0071, 0.0001, -0.0143, 0.0001, 0.0071,
	0.0582, 0.0007, -0.1177, 0.0007, 0.0582,
	0.1025, 0.0012, -0.2073, 0.0012, 0.1025,
	0.0582, 0.0007, -0.1177, 0.0007, 0.0582,
	0.0071, 0.0001, -0.0143, 0.0001, 0.0071
};
static real _5_dyy[25] = {
	0.0071, 0.0582, 0.1025, 0.0582, 0.0071,
	0.0001, 0.0007, 0.0012, 0.0007, 0.0001,
	-0.0143, -0.1177, -0.2073, -0.1177, -0.0143,
	0.0001, 0.0007, 0.0012, 0.0007, 0.0001,
	0.0071, 0.0582, 0.1025, 0.0582, 0.0071
};

/* ================================= 7x7 ==================================== */
// Non optimal 1st derivative 7x7 farid and simoncelli filter components. This is an interpolation.
static real _7_1st_p[7] = {0.004711, 0.069321, 0.245410, 0.361117, 0.245410, 0.069321, 0.004711};
static real _7_1st_d1[7] = {0.018708, 0.125376, 0.193091, 0.000000, -0.193091, -0.125376, -0.018708};
static real _7_dx[49] = {
	0.0001, 0.0006, 0.0009, 0, -0.0009, -0.0006, -0.0001,
	0.0013, 0.0087, 0.0134, 0, -0.0134, -0.0087, -0.0013,
	0.0046, 0.0308, 0.0474, 0, -0.0474, -0.0308, -0.0046,
	0.0068, 0.0453, 0.0697, 0, -0.0697, -0.0453, -0.0068,
	0.0046, 0.0308, 0.0474, 0, -0.0474, -0.0308, -0.0046,
	0.0013, 0.0087, 0.0134, 0, -0.0134, -0.0087, -0.0013,
	0.0001, 0.0006, 0.0009, 0, -0.0009, -0.0006, -0.0001
};
static real _7_dy[49] = {
	0.0001, 0.0013, 0.0046, 0.0068, 0.0046, 0.0013, 0.0001,
	0.0006, 0.0087, 0.0308, 0.0453, 0.0308, 0.0087, 0.0006,
	0.0009, 0.0134, 0.0474, 0.0697, 0.0474, 0.0134, 0.0009,
	0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000, 0.0000,
	-0.0009, -0.0134, -0.0474, -0.0697, -0.0474, -0.0134, -0.0009,
	-0.0006, -0.0087, -0.0308, -0.0453, -0.0308, -0.0087, -0.0006,
	-0.0001, -0.0013, -0.0046, -0.0068, -0.0046, -0.0013, -0.0001
};

static real _7_dxy[49] = {
	+0.0001, +0.0006, +0.0009, 0, -0.0009, -0.0006, -0.0001,
	+0.0006, +0.0041, +0.0063, 0, -0.0063, -0.0041, -0.0006,
	+0.0009, +0.0063, +0.0097, 0, -0.0097, -0.0063, -0.0009,
	+0.0000, +0.0000, +0.0000, 0, +0.0000, +0.0000, +0.0000,
	-0.0009, -0.0063, -0.0097, 0, +0.0097, +0.0063, +0.0009,
	-0.0006, -0.0041, -0.0063, 0, +0.0063, +0.0041, +0.0006,
	-0.0001, -0.0006, -0.0009, 0, +0.0009, +0.0006, +0.0001
};



// Optimal 2st derivative 7x7 farid and simoncelli filter components. Interpolation to 1st derivative.
static real _7_2st_p[7] = {0.004711, 0.069321, 0.245410, 0.361117, 0.245410, 0.069321, 0.004711};
static real _7_2st_d1[7] = {0.018708, 0.125376, 0.193091, 0.000000, -0.193091, -0.125376, -0.018708};
static real _7_2st_d2[7] = {0.055336, 0.137778, -0.056554, -0.273118, -0.056554, 0.137778, 0.055336};
static real _7_dxx[49] = {
	0.0003, 0.0006, -0.0003, -0.0013, -0.0003, 0.0006, 0.0003,
	0.0038, 0.0096, -0.0039, -0.0189, -0.0039, 0.0096, 0.0038,
	0.0136, 0.0338, -0.0139, -0.0670, -0.0139, 0.0338, 0.0136,
	0.0200, 0.0498, -0.0204, -0.0986, -0.0204, 0.0498, 0.0200,
	0.0136, 0.0338, -0.0139, -0.0670, -0.0139, 0.0338, 0.0136,
	0.0038, 0.0096, -0.0039, -0.0189, -0.0039, 0.0096, 0.0038,
	0.0003, 0.0006, -0.0003, -0.0013, -0.0003, 0.0006, 0.0003
};
static real _7_dyy[49] = {
	-0.0003, -0.0038, -0.0136, -0.0200, -0.0136, -0.0038, -0.0003,
	-0.0006, -0.0096, -0.0338, -0.0498, -0.0338, -0.0096, -0.0006,
	-0.0003, -0.0039, -0.0139, -0.0204, -0.0139, -0.0039, -0.0003,
	-0.0013, -0.0189, -0.0670, -0.0986, -0.0670, -0.0189, -0.0013,
	-0.0003, -0.0039, -0.0139, -0.0204, -0.0139, -0.0039, -0.0003,
	-0.0006, -0.0096, -0.0338, -0.0498, -0.0338, -0.0096, -0.0006,
	-0.0003, -0.0038, -0.0136, -0.0200, -0.0136, -0.0038, -0.0003
};
/* ========================================================================== */
ImpMatrix imp_filter_farid_simoncelli(int dim, char or, int comp) {
	ImpMatrix filt;

	if (dim != 5 && dim != 7) {
		msg_error("Dimension %d not avaliable.\n", dim);
	}
	if (or != 'x' && or != 'y') {
		msg_error("Orientation '%c' not avaliable.\n", or);
	}
	if (comp < 0 || comp > 2) {
		msg_error("Component %d must be:\n"
				"\t0: for full matrix.\n"
				"\t1: for left nx1 vector.\n"
				"\t2: for right 1xn vector", dim);
	}

	// <editor-fold defaultstate="collapsed" desc="5x5 filter components">
	// 5x5 full mask x|y
	if (dim == 5 && comp == 0) {
		filt.rows = dim;
		filt.columns = dim;
		filt.size = dim*dim;
		if (or == 'x') filt.data = _5_dx;
		if (or == 'y') filt.data = _5_dy;
	}

	// 5x1 mask x|y
	if (dim == 5 && comp == 1) {
		filt.rows = dim;
		filt.columns = 1;
		filt.size = dim;
		if (or == 'x') filt.data = _5_1st_p;
		if (or == 'y') filt.data = _5_1st_d1;
	}

	// 1x5 mask x|y
	if (dim == 5 && comp == 2) {
		filt.rows = 1;
		filt.columns = dim;
		filt.size = dim;
		if (or == 'x') filt.data = _5_1st_d1;
		if (or == 'y') filt.data = _5_1st_p;
	}// </editor-fold>

	// =========================================================================

	// <editor-fold defaultstate="collapsed" desc="7x7 filter components">
	// 7x7 full mask x|y
	if (dim == 7 && comp == 0) {
		filt.rows = dim;
		filt.columns = dim;
		filt.size = dim*dim;
		if (or == 'x') filt.data = _7_dx;
		if (or == 'y') filt.data = _7_dy;
	}

	// 7x1 mask x|y
	if (dim == 7 && comp == 1) {
		filt.rows = dim;
		filt.columns = 1;
		filt.size = dim;
		if (or == 'x') filt.data = _7_1st_p;
		if (or == 'y') filt.data = _7_1st_d1;
	}

	// 1x7 mask x|y
	if (dim == 7 && comp == 2) {
		filt.rows = 1;
		filt.columns = dim;
		filt.size = dim;
		if (or == 'x') filt.data = _7_1st_d1;
		if (or == 'y') filt.data = _7_1st_p;
	}// </editor-fold>

	return filt;
}
ImpMatrix imp_filter_farid_simoncelli_2st(int dim, char or, int comp) {
	ImpMatrix filt;

	if (dim != 5 && dim != 7) {
		msg_error("Dimension %d not avaliable.\n", dim);
	}
	if (or != 'x' && or != 'y') {
		msg_error("Orientation '%c' not avaliable.\n", or);
	}
	if (comp < 0 || comp > 2) {
		msg_error("Component %d must be:\n"
				"\t0: for full matrix.\n"
				"\t1: for left nx1 vector.\n"
				"\t2: for right 1xn vector", dim);
	}

	// <editor-fold defaultstate="collapsed" desc="5x5 filter components">
	// 5x5 full mask x|y
	if (dim == 5 && comp == 0) {
		filt.rows = dim;
		filt.columns = dim;
		filt.size = dim*dim;
		if (or == 'x') filt.data = _5_dxx;
		if (or == 'y') filt.data = _5_dyy;
	}

	// 5x1 mask x|y
	if (dim == 5 && comp == 1) {
		filt.rows = dim;
		filt.columns = 1;
		filt.size = dim;
		if (or == 'x') filt.data = _5_2st_p;
		if (or == 'y') filt.data = _5_2st_d2;
	}

	// 1x5 mask x|y
	if (dim == 5 && comp == 2) {
		filt.rows = 1;
		filt.columns = dim;
		filt.size = dim;
		if (or == 'x') filt.data = _5_2st_d2;
		if (or == 'y') filt.data = _5_2st_p;
	}

	// </editor-fold>

	// =========================================================================

	// <editor-fold defaultstate="collapsed" desc="7x7 filter components">
	// 7x7 full mask x|y
	if (dim == 7 && comp == 0) {
		filt.rows = dim;
		filt.columns = dim;
		filt.size = dim*dim;
		if (or == 'x') filt.data = _7_dxx;
		if (or == 'y') filt.data = _7_dyy;
	}

	// 7x1 mask x|y
	if (dim == 7 && comp == 1) {
		filt.rows = dim;
		filt.columns = 1;
		filt.size = dim;
		if (or == 'x') filt.data = _7_2st_p;
		if (or == 'y') filt.data = _7_2st_d2;
	}

	// 1x7 mask x|y
	if (dim == 7 && comp == 2) {
		filt.rows = 1;
		filt.columns = dim;
		filt.size = dim;
		if (or == 'x') filt.data = _7_2st_d2;
		if (or == 'y') filt.data = _7_2st_p;
	}// </editor-fold>

	return filt;
}
/* ========================================================================== */

ImpMatrix imp_filter_farid_simoncelli_5(char * d, ImpFilterComponent comp) {

	ImpMatrix filt;
	int dim = 5;

	if (strcmp("dx", d) == 0 || strcmp("dy", d) == 0) {

		switch (comp) {
			case IMP_FILTER_P:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _5_1st_p;
				break;
			case IMP_FILTER_D1:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _5_1st_d1;
				break;
			case IMP_FILTER_FULL:
				filt.rows = filt.columns = dim;
				filt.size = dim*dim;

				if (strcmp("dx", d) == 0) filt.data = _5_dx;
				else if (strcmp("dy", d) == 0) filt.data = _5_dy;

				break;
		}
	} else if (strcmp("dxy", d) == 0 || strcmp("dyx", d) == 0) {

		switch (comp) {
			case IMP_FILTER_FULL:
				filt.rows = filt.columns = dim;
				filt.size = dim*dim;
				filt.data = _5_dxy;
				break;
		}

	} else if (strcmp("dxx", d) == 0 || strcmp("dyy", d) == 0) {

		switch (comp) {
			case IMP_FILTER_P:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _5_2st_p;
				break;
			case IMP_FILTER_D1:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _5_2st_d1;
				break;
			case IMP_FILTER_D2:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _5_2st_d2;
				break;
			case IMP_FILTER_FULL:
				filt.rows = filt.columns = dim;
				filt.size = dim*dim;

				if (strcmp("dxx", d) == 0) filt.data = _5_dxx;
				else if (strcmp("dyy", d) == 0) filt.data = _5_dyy;

				break;
		}
	}

	return filt;

}
ImpMatrix imp_filter_farid_simoncelli_7(char * d, ImpFilterComponent comp) {

	ImpMatrix filt;
	int dim = 7;

	if (strcmp("dx", d) == 0 || strcmp("dy", d) == 0) {

		switch (comp) {
			case IMP_FILTER_P:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _7_1st_p;
				break;
			case IMP_FILTER_D1:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _7_1st_d1;
				break;
			case IMP_FILTER_FULL:
				filt.rows = filt.columns = dim;
				filt.size = dim*dim;

				if (strcmp("dx", d) == 0) filt.data = _7_dx;
				else if (strcmp("dy", d) == 0) filt.data = _7_dy;

				break;
		}
	} else if (strcmp("dxy", d) == 0 || strcmp("dyx", d) == 0) {

		switch (comp) {
			case IMP_FILTER_FULL:
				filt.rows = filt.columns = dim;
				filt.size = dim*dim;
				filt.data = _7_dxy;
				break;
		}

	} else if (strcmp("dxx", d) == 0 || strcmp("dyy", d) == 0) {

		switch (comp) {
			case IMP_FILTER_P:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _7_2st_p;
				break;
			case IMP_FILTER_D1:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _7_2st_d1;
				break;
			case IMP_FILTER_D2:
				filt.rows = dim;
				filt.columns = 1;
				filt.size = dim;
				filt.data = _7_2st_d2;
				break;
			case IMP_FILTER_FULL:
				filt.rows = filt.columns = dim;
				filt.size = dim*dim;

				if (strcmp("dxx", d) == 0) filt.data = _7_dxx;
				else if (strcmp("dyy", d) == 0) filt.data = _7_dyy;

				break;
		}
	}

	return filt;

}